﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NKH.MindSqualls;
namespace schrodinger
{
    class Program
    {
        static void Main(string[] args)
        {
            
            //default angle and speed
            double ANG_DEFAULT = 90;
            double DIST_DEFAULT = 300;
            double sPEED_DEFAULT = 100;
            double RATE_DEFAULT = 0.1;
            int BUSY_DEALY_COUNT= 5;
            int WAIT_DELAY = 100;
            int angDist = 0; // Angle in degrees or distance in centimeters.
            double spdRate = 0;
            int delayCount = 0;
            // Create a NXT brick,
            // and use Bluetooth on COM40 to communicate with it.
            NxtBrick brick = new NxtBrick(NxtCommLinkType.USB,0);

            // Create a touch sensor.
            NxtTouchSensor frontSensor = new NxtTouchSensor();
            // Create a touch sensor.
            NxtTouchSensor bellySensor = new NxtTouchSensor();
            // Create a touch sensor.
            NxtTouchSensor backSensor = new NxtTouchSensor();
            //create sensor port
            HTMotor HiTechnic = new HTMotor();

            

            
           // HiTechnic = NxtSensorPort.Port1;
            brick.Sensor1 = HiTechnic;
            brick.Sensor2 = bellySensor;
            brick.Sensor3 = backSensor;
            brick.Sensor4 = frontSensor;
            Nullable<ushort> batteryLevel;


            // Poll sensors every 50 milliseconds.
            brick.Sensor2.PollInterval = 50;
            brick.Sensor3.PollInterval = 50;
            brick.Sensor4.PollInterval = 50;

            // Connect to the NXT.
            try
            {
                brick.Connect();
                batteryLevel = brick.BatteryLevel;
                Console.WriteLine("Battery level of NXT is " + (float)batteryLevel/1000 +" V");
                Console.WriteLine( "The motor battery pack has " + HiTechnic.getVoltage() + " volts");
                byte[] sensorVersion = new byte[] { 0x02, 0x7 };
                byte [] reply = HiTechnic.SendN(sensorVersion,7 );
                Console.WriteLine("Sensor name is " + Encoding.ASCII.GetString(reply, 0, reply.Length).TrimEnd('\0'));
                HiTechnic.resetPosition();
                HiTechnic.drive(100);
                //HiTechnic.busy();
                do
                {
                    // ... and wait...
                }
                while (!((NxtTouchSensor)brick.Sensor3).IsPressed.HasValue || !((NxtTouchSensor)brick.Sensor3).IsPressed.Value);
                Console.WriteLine("Back sensor pushed");
                
            }
            catch(Exception ex)
            {
                Console.WriteLine("Faield to connect!\n"+ex.Message);
                brick.Disconnect();
            }
            Console.Read();

            // Disconnect from the NXT.
            brick.Disconnect();
        }

    }
}
